# -*- coding: utf-8 -*-
"""
Created on Tue Jul  6 11:15:49 2021

@author: sarupa
"""
## Define symbolic variable and model
from __future__ import print_function, division # Grab some handy Python3 stuff.
import mpctools as mpc
import numpy as np
import casadi
import control
from casadi import *
import matplotlib.pyplot as plt
from sklearn.decomposition import PCA
from sklearn.preprocessing import StandardScaler

## Define symbolic variable and model
# Number of variables and order
Nx = 9 # Number of states
Nu = 7 # Number of inputs 
Ny = 1 # Number of outputs
order = Nx # Number of Lie derivative from 0 to order-1


F10=1.50406459e+01
F20= 3.42104461e+00
Q1=3.14595480e+06
Q2=  1.02229466e+06
Q3=  3.49448744e+06 
Fr=1.50551530e+01
Fp=6.52273829e-01

T10=300
T20=300
V1= 4.0
V2= 4.0
V3= 4.0

E1= 50000
E2= 60000
k1= 9.972e06
k2= 9.36e06

dH1= -6*10000
dH2= -7*10000
aA= 3.5
aB= 1
aC= 0.5
Cp= 4.2
R= 8.314
rho= 1000
xA10=1.0
xA20=1.0
xB10= 0.0
xB20= 0.0
Hvap1= -3.57*10000
Hvap2= -1.57*10000
Hvap3= -4.07*10000
cm= 2

       
#system disturance
w1=0.0
w2=0.0
w3=0.0
w4=0.0
w5=0.0
w6=0.0
w7=0.0
w8=0.0
w9=0.0
# symbolic varaible
x = casadi.SX.sym('x',Nx,1) 
u = casadi.SX.sym('x',Nu,1) 
# symbolic ode model f1
f1 = casadi.SX.sym('f1',Nx,1)

xA1 = x[0]
xB1 = x[1]
T1 = x[2]
xA2 = x[3]
xB2 = x[4]
T2 = x[5]
xA3 = x[6]
xB3 = x[7]
T3 = x[8]

F10 =  u[0]
F20 =  u[1]
Q1 =  u[2]
Fr =  u[3]
Q2 =  u[4]
Q3 =  u[5]
Fp =  u[6]

xC3 = 1 - xA3 - xB3
x3a = aA*xA3 + aB*xB3 + aC*xC3
xAr = aA*xA3/x3a
xBr = aB*xB3/x3a
xCr = aC*xC3/x3a

F1 = F10 
F2 =F1 + F20

f1[0]= F10*(xA10 - xA1)/V1 + Fr/V1*(xAr-xA1) - k1*exp(-E1/(R*T1))*xA1 
f1[1]= F10*(xB10 - xB1)/V1 + Fr/V1*(xBr-xB1) + k1*exp(-E1/(R*T1))*xA1 - k2*exp(-E2/(R*T1))*xB1  
f1[2]= F10*(T10 - T1)/V1 + Fr/V1*(T3-T1) - dH1*k1*exp(-E1/(R*T1))*xA1/Cp*cm/rho - dH2*k2*exp(-E2/(R*T1))*xB1/Cp*cm/rho + Q1/(rho*Cp*V1) 
f1[3]=F1*(xA1 - xA2)/V2 + F20*(xA20 - xA2)/V2 - k1*exp(-E1/(R*T2))*xA2 
f1[4]=F1*(xB1 - xB2)/V2 + F20*(xB20 - xB2)/V2 + k1*exp(-E1/(R*T2))*xA2 - k2*exp(-E2/(R*T2))*xB2 
f1[5]=F1*(T1 - T2)/V2 + F20*(T20-T2)/V2 - dH1*k1*exp(-E1/(R*T2))*xA2/Cp *cm/rho - dH2*k2*exp(-E2/(R*T2))*xB2/Cp *cm/rho+ Q2/(rho*Cp*V2) 
f1[6]=F2*(xA2 - xA3)/V3 - (Fr+Fp)*(xAr - xA3)/V3 
f1[7]=F2*(xB2 - xB3)/V3 - (Fr+Fp)*(xBr - xB3)/V3 
f1[8]=F2*(T2 - T3)/V3 + Q3/(rho*Cp*V3)#+(Fr+Fp)*(xAr*Hvap1+xBr*Hvap2+xCr*Hvap3)/(rho*Cp*V3)*cm 

print('Symbolic model  for linear controllability is created.')

xval = np.array([9.06393325e-01, 9.32535771e-02, 3.52482083e+02, 5.80306393e-01,
       4.08395110e-01, 3.98003376e+02, 3.18882689e-01, 6.51873344e-01,
       3.99773315e+02]).T
xs = xval

x0=np.array([.25, .55, 460, .25, .6, 475, .1, 0.55, 480])*0.95

uval=np.array([1.50406459e+01, 3.42104461e+00, 3.14595480e+06, 1.50551530e+01,
       3.49448744e+06, 1.02229466e+06, 6.52273829e-01])*1.03
#uval=np.array([F10, F20, Fr, Q1, Q2, Q3, Fp])
us=uval


# In[2]:

# Linearized model and observability matrix
A_sym = casadi.SX.sym('A_sym',Nx,Nx)
A_sym = casadi.jacobian(f1,x)
A_fun = casadi.Function('A_fun',[x,u],[A_sym[:,:]])
a = A_fun(xval,uval) # Linearized A
As = np.array(a)

B_sym = casadi.SX.sym('B_sym',Nx,Nu)
B_sym = casadi.jacobian(f1,u)
B_fun = casadi.Function('B_fun',[x,u],[B_sym[:,:]])
b = B_fun(xval,uval) # Linearized A
Bs = np.array(b)

O_Lin_ss = control.ctrb(As,Bs)
Rank_O_Lin_ss = np.linalg.matrix_rank(O_Lin_ss)
print('C_Lin_ss finished.',Rank_O_Lin_ss)


# #### #Code 5: Define the CSTR model

# # In[5]:
Nsim = 50 # Number of sampling time
Nw = Nx # Number of process noise
Nv = Ny # Number of measurement noise
Delta = 0.01 # Sampling time
    
def cstrmodel(x,u):
    
    xA1 = x[0]
    xB1 = x[1]
    T1 = x[2]
    xA2 = x[3]
    xB2 = x[4]
    T2 = x[5]
    xA3 = x[6]
    xB3 = x[7]
    T3 = x[8]
        
    xC3 = 1 - xA3 - xB3
    x3a = aA*xA3 + aB*xB3 + aC*xC3
    xAr = aA*xA3/x3a
    xBr = aB*xB3/x3a
    xCr = aC*xC3/x3a
    

    
    F10 =  u[0]
    F20 =  u[1]
    Q1 =  u[2]
    Fr =  u[3]
    Q3 =  u[4]
    Q2 =  u[5]
    Fp =  u[6]
    # xA10 = u[7]
    # T20 = u[8]
    
    F1 = F10 
    F2 =F1 + F20
    
    dxdt= [ F10*(xA10 - xA1)/V1 + Fr/V1*(xAr-xA1) - k1*exp(-E1/(R*T1))*xA1 ,
         F10*(xB10 - xB1)/V1 + Fr/V1*(xBr-xB1) + k1*exp(-E1/(R*T1))*xA1 - k2*exp(-E2/(R*T1))*xB1 ,
         F10*(T10 - T1)/V1 + Fr/V1*(T3-T1) - dH1*k1*exp(-E1/(R*T1))*xA1/Cp*cm/rho - dH2*k2*exp(-E2/(R*T1))*xB1/Cp*cm/rho + Q1/(rho*Cp*V1) ,
         F1*(xA1 - xA2)/V2 + F20*(xA20 - xA2)/V2 - k1*exp(-E1/(R*T2))*xA2,
         F1*(xB1 - xB2)/V2 + F20*(xB20 - xB2)/V2 + k1*exp(-E1/(R*T2))*xA2 - k2*exp(-E2/(R*T2))*xB2 ,
         F1*(T1 - T2)/V2 + F20*(T20-T2)/V2 - dH1*k1*exp(-E1/(R*T2))*xA2/Cp *cm/rho - dH2*k2*exp(-E2/(R*T2))*xB2/Cp *cm/rho+ Q2/(rho*Cp*V2) , 
         F2*(xA2 - xA3)/V3 - (Fr+Fp)*(xAr - xA3)/V3 ,
         F2*(xB2 - xB3)/V3 - (Fr+Fp)*(xBr - xB3)/V3 ,
         F2*(T2 - T3)/V3 + Q3/(rho*Cp*V3)#+(Fr+Fp)*(xAr*Hvap1+xBr*Hvap2+xCr*Hvap3)/(rho*Cp*V3)*cm +w9
         ]
    return dxdt

F = mpc.getCasadiFunc(cstrmodel,[Nx,Nu],["x","u"],"cstrmodel")
F_rk4 = mpc.getCasadiFunc(cstrmodel,[Nx,Nu],["x","u"],"ode_rk4",rk4=True,Delta=Delta,M=1)
cstr = mpc.DiscreteSimulator(cstrmodel, Delta, [Nx,Nu], ["x","u"])

# input function

def measurement(x):
    #return np.array([x[0],x[1],x[2],x[3],x[4],x[5], x[6], x[7], x[8]])
    return x[7]
ys = measurement(xs)

# Turn into casadi functions.
H = mpc.getCasadiFunc(measurement,[Nx],["x"],"measurement")

def inputs(u):
    return u[0],u[1],u[2], u[3],u[4],u[5], u[6]
    
u_s = inputs(us)
# Turn into casadi functions.
U = mpc.getCasadiFunc(inputs,[Nu],["u"],"inputs")

print('Model is created.')


# Code : Generate test data including u(0:Nsim-1) and y(0:Nsim)
# Generate test data including u(0:Nsim-1) and y(0:Nsim)
sigma_w = 0.01
sigma_v = 0.01
v = sigma_v*np.random.randn(Nsim,Nv)
usim = np.zeros((Nsim+1,Nu))
usim[:,:] = uval

np.random.seed(100)
w=sigma_w*np.random.randn(Nsim,Nw)
w[:,2]=100*w[:,2]
w[:,5]=100*w[:,5]
w[:,8]=100*w[:,8]

# generate data x and y

xsim = np.zeros((Nsim+1,Nx))
xsim[0,:] = x0
yclean = np.zeros((Nsim, Ny))
ysim = np.zeros((Nsim, Ny))

for t in range(Nsim):
    yclean[t,:] = measurement(xsim[t,:]) # Get zero-noise measurement.
    ysim[t,:] = yclean[t,:] + v[t,:] # Add noise to measurement.
    xsim[t+1,:] = cstr.sim(xsim[t,:],usim[t,:]) + w[t,:]

print('Data is generated.')

# Code : Create the function including sensitivity analysis and variable selection

# Create the function including sensitivity analysis and variable selection


def SensAnal(size,x,y,u): 
    Sxp = np.zeros([size,Nx,Nu])
    Sup= np.zeros([size,Nx,Nu])
    dFdx = np.zeros([size,Nx,Nx])
    dHdx = np.zeros([size,Nx,Nu])
      
    x_sen = np.zeros((size,Nx))
    y_sen = np.zeros((size,Ny))
    u_sen = np.zeros((size,Nu))
    
    x_sen = x
    y_sen = y
    u_sen = u
    
    for i in range(size):
    # Sensitivity matrix calculation
        JF = mpc.util.getLinearizedModel(F_rk4, [x_sen[i,:],u_sen[i,:]],["A","B"],None)
        dFdx[i,:,:] = JF["A"]
        dHdx[i,:,:] = JF["B"]
    for i in range(size):   
        if size == 0:
            Sxp[0,:,:] =  dHdx[0,:,:]
       
        else:
            Sxp[i,:,:]=dHdx[size-i-1,:,:]
            #print(Sxp[i,:,:])
            for j in range(i):
                Sxp[i,:,:] = mpc.mtimes(dFdx[size+j-i,:,:],Sxp[i,:,:])
    
    
    # Original sensitivity matrix
    for i in range(size):
        # Concatenate sensitivity matrixes
        if i==0:
            Sa = Sxp[i,:,:]
        else:
            Sa = np.hstack((Sa,Sxp[i,:,:])) 
        
    if size==0:
        rank_t=np.linalg.matrix_rank(Sxp)
    else:
        rank_t = np.linalg.matrix_rank(Sa)
     
    # Normalization
    for i in range(size):
        for i1 in range(Nx):
            for i2 in range(Nu):
                Sxp[i,i1,i2] = Sxp[i,i1,i2]*u_sen[0,i2]/x_sen[i,i1]
        # Concatenate sensitivity matrixes
        if size==0:
            Sall=sa
        elif i==0:
            Sall = Sxp[i,:,:]
        else:
            Sall = np.concatenate((Sall,Sxp[i,:,:]), axis=0)
         
    # #'''
    pl=9
    for i in range(size):
        sl=pl*int(i)
        Sall[0+sl ,:]=0
        Sall[3+sl,:]=0
        Sall[6+sl,:]=0
        Sall[1+sl ,:]=0
        Sall[8+sl,:]=0
    
    
    U_svd, s_svd, Vh_svd = np.linalg.svd(Sall)
 
    sigma_log=np.log10(s_svd)
    
    rank = np.linalg.matrix_rank(Sall)

    return sigma_log, rank, U_svd, s_svd, Vh_svd 
    

print('The function of sensitivity evaluation and variable selection is created.')

# In[8]:

# Evaluate the ranks of O_Lie, O_Lin, and O_Sen along the trajectory
Rank_O_Lie_tra = np.zeros(Nsim)
Rank_O_Lin_tra = np.zeros(Nsim)
Rank_O_Sen_tra = np.zeros(Nsim)
for i1 in range(Nsim-1,Nsim):
    print('\nSampling time: ', i1)
    

    # Evaluate the O_Lin
    A = A_fun(xsim[i1,:],usim[i1,:]) # Linearized A
    As = np.array(A)
    
    B = B_fun(xsim[i1,:],usim[i1,:]) # Linearized A
    Bs = np.array(B)
    
    O_Lin_tra = control.ctrb(As,Bs)
    
    # Rank of O_Lin
    Rank_O_Lin_tra[i1] = np.linalg.matrix_rank(O_Lin_tra)
    print('Rank of O_Lin: ', Rank_O_Lin_tra[i1])
    
 
    Flag,rank,U_svd, s_svd, Vh_svd = SensAnal(i1,xsim[0:i1,:],ysim[0:i1,:],usim[0:i1,:])
    sigma_log=Flag

    print('Rank of O_Sen: ', rank)


t=np.arange(1,8)

plt.figure()
plt.rcParams["figure.figsize"] = [5.00,4.00]
plt.rcParams["figure.autolayout"] = True
plt.plot(t,sigma_log,'*',linewidth=2)   
plt.axhline(y = -2.43 ,color = 'r', linestyle = '--')
plt.axhline(y= -3.81, color='r',linestyle ='--')
plt.xlabel('No of singular components')
plt.ylabel('$Log_{10}$ of singular Values')
plt.title('Explained variance: Elbow graph')
plt.show()
# sudden jump observed first between 6 and 7 singular value

diff=np.zeros(6)
for i in range(6):
    diff[i]=abs(sigma_log[i]-sigma_log[i+1])
print('diff=', diff)       

###The D_j is calculated and reported based on different initial condition, input values, steady state, 
#  different system disturbance and measurement noise


# This is a sample calculation
D_j=np.zeros(9)  
D_j= (np.abs(Vh_svd[0,:]*s_svd[0])+np.abs(Vh_svd[1,:]*s_svd[1])+np.abs(Vh_svd[2,:]*s_svd[2])+
      np.abs(Vh_svd[3,:]*s_svd[3])+np.abs(Vh_svd[4,:]*s_svd[4])+np.abs(Vh_svd[5,:]*s_svd[5]))/sum(np.abs(s_svd[0:6]))
sum_vsort=np.argsort(D_j)
sum_sort=np.sort(D_j)
print("increasing order=",sum_sort)
print("increasing order=",sum_vsort)